/*
This file is part of Docking@Home.
http://docking.gcl.cis.udel.edu
Copyright (C) 2009 University of Delaware

Docking@Home is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

Docking@Home is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with Docking@Home.  If not, see <http://www.gnu.org/licenses/>.
*/
/*
	Filename: MathHelper.h
	Description: Class of math functions and data structure used within our
		program. These include vectors, matrices, quaternions, etc.
		- Math types derived from the KempoApi tMath library
	Revision History: 
		Created - 01/03/08
	Author: Robert Keller (01/03/08 -> current)
*/

#ifndef _MathHelper_h
#define _MathHelper_h

#include <math.h>

// ------------------------------------------------------------
// Data Types
// ------------------------------------------------------------

typedef union Tuple2f_t
{
    struct
    {
        float X, Y;
    } s;

    float T[2];
} Tuple2fT;      //A generic 2-element tuple that is represented by single-precision floating point x,y coordinates. 

typedef union Tuple3f_t
{
    struct
    {
        float X, Y, Z;
    } s;

    float T[3];
} Tuple3fT;      //A generic 3-element tuple that is represented by single precision-floating point x,y,z coordinates. 

typedef union Tuple4f_t
{
    struct
    {
        float X, Y, Z, W;
    } s;

    float T[4];
} Tuple4fT;      //A 4-element tuple represented by single-precision floating point x,y,z,w coordinates. 

typedef union Matrix3f_t
{
        struct
        {
            //column major
            union { float M00; float XX; float SX; };  //XAxis.X and Scale X
            union { float M10; float XY;             };  //XAxis.Y
            union { float M20; float XZ;             };  //XAxis.Z
            union { float M01; float YX;             };  //YAxis.X
            union { float M11; float YY; float SY; };  //YAxis.Y and Scale Y
            union { float M21; float YZ;             };  //YAxis.Z
            union { float M02; float ZX;             };  //ZAxis.X
            union { float M12; float ZY;             };  //ZAxis.Y
            union { float M22; float ZZ; float SZ; };  //ZAxis.Z and Scale Z
        } s;
        float M[9];
} Matrix3fT;     //A single precision floating point 3 by 3 matrix. 

typedef union Matrix4f_t
{
        struct
        {
            //column major
            union { float M00; float XX; float SX; };  //XAxis.X and Scale X
            union { float M10; float XY;             };  //XAxis.Y
            union { float M20; float XZ;             };  //XAxis.Z
            union { float M30; float XW;             };  //XAxis.W
            union { float M01; float YX;             };  //YAxis.X
            union { float M11; float YY; float SY; };  //YAxis.Y and Scale Y
            union { float M21; float YZ;             };  //YAxis.Z
            union { float M31; float YW;             };  //YAxis.W
            union { float M02; float ZX;             };  //ZAxis.X
            union { float M12; float ZY;             };  //ZAxis.Y
            union { float M22; float ZZ; float SZ; };  //ZAxis.Z and Scale Z
            union { float M32; float ZW;             };  //ZAxis.W
            union { float M03; float TX;             };  //Trans.X
            union { float M13; float TY;             };  //Trans.Y
            union { float M23; float TZ;             };  //Trans.Z
            union { float M33; float TW; float SW; };  //Trans.W and Scale W
        } s;
        float M[16];
} Matrix4fT;     //A single precision floating point 4 by 4 matrix. 

//"Inherited" types
#define Point2fT    Tuple2fT   //A 2 element point that is represented by single precision floating point x,y coordinates. 

#define Quat4fT     Tuple4fT   //A 4 element unit quaternion represented by single precision floating point x,y,z,w coordinates. 

#define Vector2fT   Tuple2fT   //A 2-element vector that is represented by single-precision floating point x,y coordinates. 
#define Vector3fT   Tuple3fT   //A 3-element vector that is represented by single-precision floating point x,y,z coordinates. 

//Custom math, or speed overrides
#define FuncSqrt    sqrtf

//utility macros
//assuming IEEE-754(float), which i believe has max precision of 7 bits
# define Epsilon 1.0e-5

// ------------------------------------------------------------
// Point Functions
// ------------------------------------------------------------

/**
 * Sets the value of this tuple to the vector sum of itself and tuple t1.
 * @param t1  the other tuple
 */
inline static void Point2fAdd(Point2fT* NewObj, const Tuple2fT* t1)
{
	NewObj->s.X += t1->s.X;
    NewObj->s.Y += t1->s.Y;
}

/**
 * Sets the value of this tuple to the vector difference of itself 
 * and tuple t1 (this = this - t1).
 * @param t1 the other tuple
 */
inline static void Point2fSub(Point2fT* NewObj, const Tuple2fT* t1)
{
    NewObj->s.X -= t1->s.X;
    NewObj->s.Y -= t1->s.Y;
}

// ------------------------------------------------------------
// Vector Functions
// ------------------------------------------------------------

/**
 * Sets this vector to be the vector cross product of vectors v1 and v2.
 * @param v1 the first vector
 * @param v2 the second vector
 */
inline static void Vector3fCross(Vector3fT* NewObj, const Vector3fT* v1, 
		const Vector3fT* v2)
{
    Vector3fT Result; //safe not to initialize

    // store on stack once for aliasing-safty
    // i.e. safe when a.cross(a, b)

    Result.s.X = (v1->s.Y * v2->s.Z) - (v1->s.Z * v2->s.Y);
    Result.s.Y = (v1->s.Z * v2->s.X) - (v1->s.X * v2->s.Z);
    Result.s.Z = (v1->s.X * v2->s.Y) - (v1->s.Y * v2->s.X);

    //copy result back
    *NewObj = Result;
}

/**
    * Computes the dot product of the this vector and vector v1.
    * @param  v1 the other vector
    */
inline
static float Vector3fDot(const Vector3fT* NewObj, const Vector3fT* v1)
{
    return  (NewObj->s.X * v1->s.X) +
            (NewObj->s.Y * v1->s.Y) +
            (NewObj->s.Z * v1->s.Z);
}

/**
    * Returns the squared length of this vector.
    * @return the squared length of this vector
    */
inline
static float Vector3fLengthSquared(const Vector3fT* NewObj)
{
    return  (NewObj->s.X * NewObj->s.X) +
            (NewObj->s.Y * NewObj->s.Y) +
            (NewObj->s.Z * NewObj->s.Z);
}

/**
    * Returns the length of this vector.
    * @return the length of this vector
    */
inline
static float Vector3fLength(const Vector3fT* NewObj)
{
    return FuncSqrt(Vector3fLengthSquared(NewObj));
}

// ------------------------------------------------------------
// Matrix Functions
// ------------------------------------------------------------

inline
static void Matrix3fSetZero(Matrix3fT* NewObj)
{
    NewObj->s.M00 = NewObj->s.M01 = NewObj->s.M02 = 
    NewObj->s.M10 = NewObj->s.M11 = NewObj->s.M12 = 
    NewObj->s.M20 = NewObj->s.M21 = NewObj->s.M22 = 0.0f;
}

/**
    * Sets this Matrix3 to identity.
    */
inline
static void Matrix3fSetIdentity(Matrix3fT* NewObj)
{
    Matrix3fSetZero(NewObj);

    //then set diagonal as 1
    NewObj->s.M00 = 
    NewObj->s.M11 = 
    NewObj->s.M22 = 1.0f;
}

/**
    * Sets the value of this matrix to the matrix conversion of the
    * quaternion argument. 
    * @param q1 the quaternion to be converted 
    */
//$hack this can be optimized some(if s == 0)
inline
static void Matrix3fSetRotationFromQuat4f(Matrix3fT* NewObj, const Quat4fT* q1)
{
    float n, s;
    float xs, ys, zs;
    float wx, wy, wz;
    float xx, xy, xz;
    float yy, yz, zz;

    n = (q1->s.X * q1->s.X) + (q1->s.Y * q1->s.Y) + (q1->s.Z * q1->s.Z) + (q1->s.W * q1->s.W);
    s = (n > 0.0f) ? (2.0f / n) : 0.0f;

    xs = q1->s.X * s;  ys = q1->s.Y * s;  zs = q1->s.Z * s;
    wx = q1->s.W * xs; wy = q1->s.W * ys; wz = q1->s.W * zs;
    xx = q1->s.X * xs; xy = q1->s.X * ys; xz = q1->s.X * zs;
    yy = q1->s.Y * ys; yz = q1->s.Y * zs; zz = q1->s.Z * zs;

    NewObj->s.XX = 1.0f - (yy + zz); NewObj->s.YX =         xy - wz;  NewObj->s.ZX =         xz + wy;
    NewObj->s.XY =         xy + wz;  NewObj->s.YY = 1.0f - (xx + zz); NewObj->s.ZY =         yz - wx;
    NewObj->s.XZ =         xz - wy;  NewObj->s.YZ =         yz + wx;  NewObj->s.ZZ = 1.0f - (xx + yy);
}

/**
    * Sets the value of this matrix to the result of multiplying itself
    * with matrix m1. 
    * @param m1 the other matrix 
    */
inline
static void Matrix3fMulMatrix3f(Matrix3fT* NewObj, const Matrix3fT* m1)
{
    Matrix3fT Result; //safe not to initialize

    // alias-safe way.
    Result.s.M00 = (NewObj->s.M00 * m1->s.M00) + (NewObj->s.M01 * m1->s.M10) + (NewObj->s.M02 * m1->s.M20);
    Result.s.M01 = (NewObj->s.M00 * m1->s.M01) + (NewObj->s.M01 * m1->s.M11) + (NewObj->s.M02 * m1->s.M21);
    Result.s.M02 = (NewObj->s.M00 * m1->s.M02) + (NewObj->s.M01 * m1->s.M12) + (NewObj->s.M02 * m1->s.M22);

    Result.s.M10 = (NewObj->s.M10 * m1->s.M00) + (NewObj->s.M11 * m1->s.M10) + (NewObj->s.M12 * m1->s.M20);
    Result.s.M11 = (NewObj->s.M10 * m1->s.M01) + (NewObj->s.M11 * m1->s.M11) + (NewObj->s.M12 * m1->s.M21);
    Result.s.M12 = (NewObj->s.M10 * m1->s.M02) + (NewObj->s.M11 * m1->s.M12) + (NewObj->s.M12 * m1->s.M22);

    Result.s.M20 = (NewObj->s.M20 * m1->s.M00) + (NewObj->s.M21 * m1->s.M10) + (NewObj->s.M22 * m1->s.M20);
    Result.s.M21 = (NewObj->s.M20 * m1->s.M01) + (NewObj->s.M21 * m1->s.M11) + (NewObj->s.M22 * m1->s.M21);
    Result.s.M22 = (NewObj->s.M20 * m1->s.M02) + (NewObj->s.M21 * m1->s.M12) + (NewObj->s.M22 * m1->s.M22);

    //copy result back to this
    *NewObj = Result;
}

inline
static void Matrix4fSetRotationScaleFromMatrix4f(Matrix4fT* NewObj, const Matrix4fT* m1)
{
    NewObj->s.XX = m1->s.XX; NewObj->s.YX = m1->s.YX; NewObj->s.ZX = m1->s.ZX;
    NewObj->s.XY = m1->s.XY; NewObj->s.YY = m1->s.YY; NewObj->s.ZY = m1->s.ZY;
    NewObj->s.XZ = m1->s.XZ; NewObj->s.YZ = m1->s.YZ; NewObj->s.ZZ = m1->s.ZZ;
}

/**
    * Performs SVD on this matrix and gets scale and rotation.
    * Rotation is placed into rot3, and rot4.
    * @param rot3 the rotation factor(Matrix3d). if null, ignored
    * @param rot4 the rotation factor(Matrix4) only upper 3x3 elements are changed. if null, ignored
    * @return scale factor
    */
inline
static float Matrix4fSVD(const Matrix4fT* NewObj, Matrix3fT* rot3, Matrix4fT* rot4)
{
    float s, n;

    // this is a simple svd.
    // Not complete but fast and reasonable.
    // See comment in Matrix3d.

    s = FuncSqrt(
            ( (NewObj->s.XX * NewObj->s.XX) + (NewObj->s.XY * NewObj->s.XY) + (NewObj->s.XZ * NewObj->s.XZ) + 
                (NewObj->s.YX * NewObj->s.YX) + (NewObj->s.YY * NewObj->s.YY) + (NewObj->s.YZ * NewObj->s.YZ) +
                (NewObj->s.ZX * NewObj->s.ZX) + (NewObj->s.ZY * NewObj->s.ZY) + (NewObj->s.ZZ * NewObj->s.ZZ) ) / 3.0f );

    if (rot3)   //if pointer not null
    {
        //this->getRotationScale(rot3);
        rot3->s.XX = NewObj->s.XX; rot3->s.XY = NewObj->s.XY; rot3->s.XZ = NewObj->s.XZ;
        rot3->s.YX = NewObj->s.YX; rot3->s.YY = NewObj->s.YY; rot3->s.YZ = NewObj->s.YZ;
        rot3->s.ZX = NewObj->s.ZX; rot3->s.ZY = NewObj->s.ZY; rot3->s.ZZ = NewObj->s.ZZ;

        // zero-div may occur.

        n = 1.0f / FuncSqrt( (NewObj->s.XX * NewObj->s.XX) +
                                    (NewObj->s.XY * NewObj->s.XY) +
                                    (NewObj->s.XZ * NewObj->s.XZ) );
        rot3->s.XX *= n;
        rot3->s.XY *= n;
        rot3->s.XZ *= n;

        n = 1.0f / FuncSqrt( (NewObj->s.YX * NewObj->s.YX) +
                                    (NewObj->s.YY * NewObj->s.YY) +
                                    (NewObj->s.YZ * NewObj->s.YZ) );
        rot3->s.YX *= n;
        rot3->s.YY *= n;
        rot3->s.YZ *= n;

        n = 1.0f / FuncSqrt( (NewObj->s.ZX * NewObj->s.ZX) +
                                    (NewObj->s.ZY * NewObj->s.ZY) +
                                    (NewObj->s.ZZ * NewObj->s.ZZ) );
        rot3->s.ZX *= n;
        rot3->s.ZY *= n;
        rot3->s.ZZ *= n;
    }

    if (rot4)   //if pointer not null
    {
        if (rot4 != NewObj)
        {
            Matrix4fSetRotationScaleFromMatrix4f(rot4, NewObj);  // private method
        }

        // zero-div may occur.

        n = 1.0f / FuncSqrt( (NewObj->s.XX * NewObj->s.XX) +
                                    (NewObj->s.XY * NewObj->s.XY) +
                                    (NewObj->s.XZ * NewObj->s.XZ) );
        rot4->s.XX *= n;
        rot4->s.XY *= n;
        rot4->s.XZ *= n;

        n = 1.0f / FuncSqrt( (NewObj->s.YX * NewObj->s.YX) +
                                    (NewObj->s.YY * NewObj->s.YY) +
                                    (NewObj->s.YZ * NewObj->s.YZ) );
        rot4->s.YX *= n;
        rot4->s.YY *= n;
        rot4->s.YZ *= n;

        n = 1.0f / FuncSqrt( (NewObj->s.ZX * NewObj->s.ZX) +
                                    (NewObj->s.ZY * NewObj->s.ZY) +
                                    (NewObj->s.ZZ * NewObj->s.ZZ) );
        rot4->s.ZX *= n;
        rot4->s.ZY *= n;
        rot4->s.ZZ *= n;
    }

    return s;
}

inline
static void Matrix4fSetRotationScaleFromMatrix3f(Matrix4fT* NewObj, const Matrix3fT* m1)
{
    NewObj->s.XX = m1->s.XX; NewObj->s.YX = m1->s.YX; NewObj->s.ZX = m1->s.ZX;
    NewObj->s.XY = m1->s.XY; NewObj->s.YY = m1->s.YY; NewObj->s.ZY = m1->s.ZY;
    NewObj->s.XZ = m1->s.XZ; NewObj->s.YZ = m1->s.YZ; NewObj->s.ZZ = m1->s.ZZ;
}

inline
static void Matrix4fMulRotationScale(Matrix4fT* NewObj, float scale)
{
    NewObj->s.XX *= scale; NewObj->s.YX *= scale; NewObj->s.ZX *= scale;
    NewObj->s.XY *= scale; NewObj->s.YY *= scale; NewObj->s.ZY *= scale;
    NewObj->s.XZ *= scale; NewObj->s.YZ *= scale; NewObj->s.ZZ *= scale;
}

/**
    * Sets the rotational component (upper 3x3) of this matrix to the matrix
    * values in the T precision Matrix3d argument; the other elements of
    * this matrix are unchanged; a singular value decomposition is performed
    * on this object's upper 3x3 matrix to factor out the scale, then this
    * object's upper 3x3 matrix components are replaced by the passed rotation
    * components, and then the scale is reapplied to the rotational
    * components.
    * @param m1 T precision 3x3 matrix
    */
inline
static void Matrix4fSetRotationFromMatrix3f(Matrix4fT* NewObj, const Matrix3fT* m1)
{
    float scale;

    scale = Matrix4fSVD(NewObj, NULL, NULL);

    Matrix4fSetRotationScaleFromMatrix3f(NewObj, m1);
    Matrix4fMulRotationScale(NewObj, scale);
}

inline 
static Matrix4fT matrix4fTSetValues(float M00, float M10, float M20, 
	float M30, float M01, float M11, float M21, float M31, float M02, 
	float M12, float M22, float M32, float M03, float M13, float M23, 
	float M33)
{
	Matrix4fT temp = {M00, M10, M20, M30,
					  M01, M11, M21, M31,
					  M02, M12, M22, M32,
					  M03, M13, M23, M33};

	return temp;
}// matrix4fTSetValues

inline 
static Matrix3fT matrix3fTSetValues(float M00, float M10, float M20, 
	float M01, float M11, float M21, float M02, float M12, float M22)
{
	Matrix3fT temp = {M00, M10, M20,
					  M01, M11, M21,
					  M02, M12, M22};

	return temp;
}// matrix3fTSetValues

#endif
